#include "sensor_processor.h"

SensorProcessor::SensorProcessor(tf2_ros::Buffer* tf_buffer):
    __tf_buffer(tf_buffer),__scan_ptr(nullptr){
    __base_scan_pub = __nh.advertise<sensor_msgs::LaserScan>("/base_scan", 1);
    __scan_sub = __nh.subscribe("/scan_raw", 10, &SensorProcessor::LaserScanCallback, this);
}


SensorProcessor::~SensorProcessor(){

}


/**************************************************************
 * Pub
 ***************************************************************/
// 发布LidarScan2D
void SensorProcessor::PublishLidarScan2D(LidarScan2D lidar_scan_2d){
    sensor_msgs::LaserScan scan_msg;
    scan_msg.header.frame_id = lidar_scan_2d.frame_id;
    scan_msg.header.stamp = lidar_scan_2d.time_stamp;
    scan_msg.angle_min = lidar_scan_2d.angle_min;
    scan_msg.angle_max = lidar_scan_2d.angle_max;
    scan_msg.angle_increment = lidar_scan_2d.angle_increment;
    scan_msg.time_increment = lidar_scan_2d.time_increment;
    scan_msg.scan_time = lidar_scan_2d.scan_time;
    scan_msg.range_min = lidar_scan_2d.range_min;
    scan_msg.range_max = lidar_scan_2d.range_max;
    for(auto lidar_2d : lidar_scan_2d.scan_data){
        scan_msg.ranges.emplace_back(lidar_2d.range);
        scan_msg.intensities.emplace_back(lidar_2d.intensitie);

        scan_msg.range_min = min(lidar_scan_2d.range_min, lidar_2d.range);
        scan_msg.range_max = max(lidar_scan_2d.range_min, lidar_2d.range);
    }
    __base_scan_pub.publish(scan_msg);
}

// 激光雷达回调函数
void SensorProcessor::LaserScanCallback(const sensor_msgs::LaserScanConstPtr& msg){
    __scan_ptr = msg;
}


// 雷达数据转换 sensor_msgs::LaserScan -> LidarScan2D
bool SensorProcessor::Scan2LidarScan2D(LidarScan2D& lidatr_scan_2d){
    if(!__scan_ptr) return false;
    string base_frame = "base_link";
    geometry_msgs::PoseStamped base2laser_link;
    if(!core::GetTFPose(__tf_buffer, base2laser_link, base_frame, __scan_ptr->header.frame_id, ros::Time::now())){
        return false;
    }
    ROS_INFO("base2laser_link, x=%0.3f, y=%0.3f, yaw=%0.3f.", 
        base2laser_link.pose.position.x, 
        base2laser_link.pose.position.y,
        tf2::getYaw(base2laser_link.pose.orientation)
    );
    tf2::Transform base2laser_tf;
    tf2::convert(base2laser_link.pose, base2laser_tf);

    // int lidar_nums = 720;
    // lidatr_scan_2d.frame_id = base_frame;
    // lidatr_scan_2d.time_stamp = __scan_ptr->header.stamp;
    // lidatr_scan_2d.angle_min = -M_PI;
    // lidatr_scan_2d.angle_max = M_PI;
    // lidatr_scan_2d.angle_increment = (lidatr_scan_2d.angle_max - lidatr_scan_2d.angle_min) / (lidar_nums - 1);
    
    lidatr_scan_2d.frame_id = base_frame;
    lidatr_scan_2d.time_stamp = __scan_ptr->header.stamp;
    lidatr_scan_2d.angle_min = __scan_ptr->angle_min;
    lidatr_scan_2d.angle_max = __scan_ptr->angle_max;
    lidatr_scan_2d.angle_increment = __scan_ptr->angle_increment;
    lidatr_scan_2d.time_increment = __scan_ptr->time_increment;
    lidatr_scan_2d.scan_time = __scan_ptr->scan_time;
    lidatr_scan_2d.range_min = __scan_ptr->range_min;
    lidatr_scan_2d.range_max = __scan_ptr->range_max;
    for(int i=0; i<__scan_ptr->ranges.size(); ++i){
        // 雷达原始坐标系
        float angle = __scan_ptr->angle_min + i*__scan_ptr->angle_increment;
        float laser_x = __scan_ptr->ranges[i] * cos(angle);
        float laser_y = __scan_ptr->ranges[i] * sin(angle);
        // tf2::Transform map_pose = latest_tf_.inverse() * odom_pose_tf2;
        tf2::Vector3 point_in(laser_x, laser_y, 0);
        tf2::Vector3 point_out = base2laser_tf * point_in;
        // 转换为机器人坐标系
        float base_x = point_out.getX();
        float base_y = point_out.getY();
        // 构造Lidar2D
        Lidar2D lidar_2d;
        lidar_2d.idx = i;
        lidar_2d.angle = atan2(base_y, base_x);
        lidar_2d.range = hypot(base_x, base_y);
        lidar_2d.laser_pose.x = base_x;
        lidar_2d.laser_pose.y = base_y;
        if(!__scan_ptr->intensities.empty())
            lidar_2d.intensitie = __scan_ptr->intensities[i];
        lidar_2d.stamp = __scan_ptr->header.stamp + ros::Duration(__scan_ptr->time_increment);
        lidatr_scan_2d.scan_data.emplace_back(lidar_2d);
        PublishLidarScan2D(lidatr_scan_2d);
    }  
    return true;
}